#include <cv.h>
#include <highgui.h>

#include "HandInterface.hpp"

#include <boost/thread.hpp>

using namespace cv;
using namespace hi;

SingleTrackerFaceActivationRunner::SingleTrackerFaceActivationRunner( Ptr< AbstractTrackDataServer > trackDataServerPtr ) : trackDataServerPtr( trackDataServerPtr ),
hl(0), ht(0), nextFrameReadyBar(3), computationCompleteBar(3), running(false), runningFaceTracker(false) {
};

SingleTrackerFaceActivationRunner::~SingleTrackerFaceActivationRunner() {
}

int SingleTrackerFaceActivationRunner::init() {
	HiVisualiser::addWindow("handtracker");
	HiVisualiser::moveWindow("handtracker",500,200);
	
	capPtr = Ptr< VideoCapture >(new VideoCapture(0)); // open the default camera
	bool captureSizeSet = capPtr->set( CV_CAP_PROP_FRAME_WIDTH , 640 );
	captureSizeSet &= capPtr->set( CV_CAP_PROP_FRAME_HEIGHT , 480 );
	if(!capPtr->isOpened() || !captureSizeSet) { // check if we succeeded
		return -1;
	} else {
		return 0;
	}
}

void SingleTrackerFaceActivationRunner::run() {
	running = true;
	
	while (running && localise() == -1 ) {}
	if (!running) return;
	
	//prepare hand classifier
	Ptr< GeometricGestureClassifier > internalClassifierPtr = Ptr< GeometricGestureClassifier >( new BoostGestureClassifier( "C0", "C0", hl->getProbabilityTransformer() ) );
	
	//init handtracker with paramset3
	ht = HandTracker::init( false, 10.0, 0.1, 100.0, 10.0,
						   trackingRect, frame, internalClassifierPtr );
	
	// -- run hand tracker -- //
	boost::thread handTrackerThread( boost::bind( &SingleTrackerFaceActivationRunner::runHandTracker, this ) );
	
	// -- run face tracker -- //
	//prepare face tracker
	ft = new FaceTracker( hl->getProbabilityTransformer() );
	faceTrackingRect = trackingRect; //copy rect
	runningFaceTracker = true;
	boost::thread faceTrackerThread( boost::bind( &SingleTrackerFaceActivationRunner::runFaceTracker, this ) );
	
	Rect trackingRectBuffer;
	Rect faceTrackingRectBuffer;
	bool isHandActiveBuffer;
	bool initialDeactivation = true;
	
	while (running) {
		trackingRectBuffer = trackingRect;
		faceTrackingRectBuffer = faceTrackingRect;
		isHandActiveBuffer = ht->isHandActive();
		setNextFrame();
		nextFrameReadyBar.wait(); //allow tracking of next rect to start while processing data from previous rect
		
		//tracking post-processing and data queue for send
		if (initialDeactivation) {
			initialDeactivation = rectanglesIntersect( trackingRectBuffer, faceTrackingRectBuffer ); //disable tracker while on face
			isHandActiveBuffer = false;
		} else {
			runningFaceTracker = false;
		}

		
		trackDataServerPtr->putTrackData( isHandActiveBuffer, trackingRectBuffer.x+trackingRectBuffer.width/2, trackingRectBuffer.y+trackingRectBuffer.height/2 );

		computationCompleteBar.wait(); //wait for tracking to complete
		HiVisualiser::refreshWindow("handtracker");
		waitKey(1);//helps visualiser run more smoothly in some very strange way
	}
}

void SingleTrackerFaceActivationRunner::runFaceTracker() {
	while (runningFaceTracker) {
		nextFrameReadyBar.wait();
		
		ft->track( frame, faceTrackingRect );
		
		//VISUALISATION
		if ( HiVisualiser::windowMatrixMap.find("handtracker") != HiVisualiser::windowMatrixMap.end() && !HiVisualiser::windowMatrixMap["handtracker"].empty()) {
			rectangle( HiVisualiser::windowMatrixMap["handtracker"], Point( faceTrackingRect.x, faceTrackingRect.y ),
					  Point( faceTrackingRect.x + faceTrackingRect.width, faceTrackingRect.y + faceTrackingRect.height ),
					  Scalar(255,0,0) );
		}
		
		computationCompleteBar.wait();
	}
	
	nextFrameReadyBar.decrement();
	computationCompleteBar.decrement();
}

void SingleTrackerFaceActivationRunner::runHandTracker() {
	while (running) {
		nextFrameReadyBar.wait();
		
		trackingRect = ht->track( frame );
		
		computationCompleteBar.wait();
	}
}

void SingleTrackerFaceActivationRunner::stop() {
	runningFaceTracker = false;
	running = false;
}

// -- protected methods -- //
void SingleTrackerFaceActivationRunner::setNextFrame() {
	(*capPtr) >> frame;
	flip(frame, frame, 1);
	
	//VISUALISATION //TODO RAT : for visualisation only!
	Mat imgToDraw;
	frame.copyTo( imgToDraw );
	HiVisualiser::windowMatrixMap["handtracker"] = imgToDraw;
}

int SingleTrackerFaceActivationRunner::localise() {
	hl = new HandLocaliser();
	try {
		setNextFrame();
		trackingRect = hl->localiseFace( frame );
		//trackingRect = hl->localiseHand( frame );
		cout << "Face found" << endl;
	}
	catch (cv::Exception e) {
		if ( e.code == HI_ERR_NOFACEFOUND ) {
			return -1;
		} else {
			throw;
		}
	}
	
	//forcing rectangle to stay inside frame
	forceInside( frame.size(), trackingRect );
	
	return 0;
}


